/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include "RangingFileReader.hpp"
CRangingFileReader::CRangingFileReader()
 :iFileReaded(false),
  iData(),
  iTimes(),
  iBegin(),
  iEnd(),
  iMaCIGroup(""),
  iInstanceName(""),
  iDeviceLimits(),
  iDevicePosition()
{


}
//*****************************************************************************
/**
   * Destructor
   */
CRangingFileReader::~CRangingFileReader()
{
 for(EACH_IN_i(iData)){

    delete(i->second);
   
  }
   iData.clear();
   iTimes.clear();

}
//*****************************************************************************

/**
   * Opens file with wanted filename
   */
bool CRangingFileReader::ReadFile(std::string aFileName)
{
  if(iFileReaded){
    for(EACH_IN_i(iData)){
      delete(i->second);
    }
    iData.clear();
    iTimes.clear();
  }

  dPrint(1,"Opening file '%s'",aFileName.c_str());
  FILE *file= fopen(aFileName.c_str(),"rt");
	if(file == NULL){
		dPrint(1,"Cannot open '%s'",aFileName.c_str());
		return false;
	}
	
  char c[200];
  char maciGroupName[256];
  char interfaceName[64];
  std::string temp;
  //Read the four first lines..and get the interfacename...
  fgets (c, 200,file );

  fscanf(file,"%s %s %s %s",c,maciGroupName,c,interfaceName);
  iInstanceName = std::string(interfaceName);
  iMaCIGroup = std::string(maciGroupName);
  fgets (c, 200,file );
  fgets (c, 200,file );
  fgets (c, 200,file );
  dPrint(1,"%s",c);

  std::string additionalInformation(c);
  //Search for device limits
  std::string::size_type loc = additionalInformation.find( "maxDistance:", 0 );
  std::istringstream stream;
  std::string s(c);
  stream.str(s);
  if( loc != std::string::npos ) {

    float maxDistance,minDistance;
    stream >> temp >>maxDistance >>temp >>temp>> minDistance;
    iDeviceLimits.max_distance = maxDistance;
    iDeviceLimits.min_distance = minDistance;
  }else{

    dPrint(1,"Did not found device limits in the file");

  }

  //Search for device position
  loc = additionalInformation.find( "positionX", 0 );
  if( loc != std::string::npos ) {
    float x=1,y=1.1,z=1.1,heading=1.1;
    stream >>temp>>x>>temp>>temp>>y>>temp>>temp>>z>>temp >>temp >>heading;
    dPrint(10,"got device position (%f, %f, %f, %f)",x,y,z,heading);
    iDevicePosition.x = x;
    iDevicePosition.y = y;
    iDevicePosition.z = z;
    iDevicePosition.heading = heading;

  }else{
    dPrint(1,"Did not find device position");
  }
  iFileReaded = ReadData(file);
  return iFileReaded;


}
//*****************************************************************************

bool CRangingFileReader::GetServiceInformation(std::string &aMaCIGroup,
                                               std::string &aInstanceName)
{
  if(iFileReaded){
    aMaCIGroup = iMaCIGroup;
    aInstanceName = iInstanceName;
  }else{
    dPrint(1,"file not readed, could not give service information");
  }

  return iFileReaded;


}
//*****************************************************************************

bool CRangingFileReader::GetDeviceLimits(MaCI::Ranging::TDeviceInformationLimits &aLimits)
{
  
  if(iFileReaded){
    aLimits= iDeviceLimits;
    return true;
  }else{
    dPrint(1,"File not read, could not give device limits");
    return false;
  }
}
//*****************************************************************************

bool CRangingFileReader::GetDevicePosition(MaCI::Ranging::TDeviceInformationPosition &aPosition)
{
  if(iFileReaded){
    aPosition = iDevicePosition;
    return true;
  }else{
    dPrint(1,"File not read, could not give device position");
    return false;
  }

}
//*****************************************************************************

bool CRangingFileReader::GetTimes(std::list<gim::time> &aTimes)
{
  if(iFileReaded){
    aTimes.assign(iTimes.begin(),iTimes.end());
    return true;
  }else{
    dPrint(1,"File not read, could not give times");
    return false;
  }

}
//*****************************************************************************
bool CRangingFileReader::GetTimeLimits(gim::time &aBegin,gim::time &aEnd)
{
  if(iFileReaded){
    aBegin = iBegin;
    aEnd = iEnd;
    return true;
  }else{
     dPrint(1,"File not read, could not give time limits");
    return false;
  }

}
//*****************************************************************************
 
 bool CRangingFileReader::GetAllData(TRangingDataMap &aData)
{
  if(iFileReaded){
    aData.insert(iData.begin(),iData.end());
    return true;
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }

}
//*****************************************************************************

bool CRangingFileReader::GetDistanceArray(MaCI::Ranging::CRangingData &aData,
                                          gim::time aTime)
{
  if(iFileReaded){
    std::map<gim::time,MaCI::Ranging::CRangingData *>::iterator iter
      =iData.find(aTime);
   
   if(iter != iData.end()){
     aData = (*(iter->second));
     return true;
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
  }else{
   dPrint(1,"File not read, could not give data");
    return false;
  }
  
}
//*****************************************************************************
bool CRangingFileReader::ReadData(FILE *aFile){
  using namespace MaCI::Ranging;
  MaCI::Common::TTimestamp timestamp;
  float beam_width = 0.0,beam_count = 0.0;
  std::vector<float> angles;
  std::vector<float> distances;
  float angle = 0.0,distance = 0.0;
  int number = 1;
  bool first = true;
  while(number != EOF){
    
    angles.clear();
    distances.clear();
    number = 1;
    number = fscanf(aFile,"%u %u %f %f",
                    &(timestamp.timestamp_s),
                    &(timestamp.timestamp_us),
                    &beam_width,
                    &beam_count);
    if(number == EOF){
      break;
    }   
    if(first && number == 4){
      iBegin = timestamp.GetGimTime();
      first = false;
    }
    //    dPrint(1,"timestamp %s, number %d",timestamp.GetGimTime().toString().c_str(),number);

    for(unsigned j = 0; j< beam_count;j++){
      number = fscanf(aFile,"%f %f",
                      &angle,
                      &distance);

      angles.push_back(angle);
      distances.push_back(distance);

    }

    if(angles.size() >0){
      MaCI::Ranging::CRangingData *rdata = new MaCI::Ranging::CRangingData();
      rdata->CreateInternalBinBag();
      rdata->SetTimestamp(timestamp);
      
      rdata->SetDistanceHeader(MaCI::Ranging::TDistanceHeader(beam_width,beam_count));
      for(unsigned int i = 0; i< angles.size(); i++){
        rdata->AddDistance(MaCI::Ranging::TDistance(angles[i], distances[i]));
        
      }
      
      iData[timestamp.GetGimTime()] = rdata;
      iTimes.push_back(timestamp.GetGimTime());
    
    }
    
    
  }
  if(!first){
    iEnd = timestamp.GetGimTime();
  }
  
  if(!first){
    
    return true;
  }else{
    return false;
  }
}
